#include <constants.h>
#include "fnavfn/PlotTrajectory.h"
#include "fnavfn/Clear.h"

class ControlCommand{
	private:
		double _lv;
		double _av;
		int n_steps;
	public:
		ControlCommand(double xv,double tv,int steps);
		ControlCommand();
		double lv(double lv=INF);
		double av(double av=INF);
		int number_steps();
};

class Trajectory{
	private:
		std::vector<double> x,y,t;
		ControlCommand ctrl;  
		ros::NodeHandle n;
		ros::ServiceClient client, clr;		
		fnavfn::PlotTrajectory srv;
		fnavfn::Clear sclr;
		double cost;
		
	public:
		Trajectory(ControlCommand &cc);
		Trajectory();
		double get_cost();
		int length();
		double add_cost(double c);
		
		void add_point(double hpos,double vpos,double theta);
		TFFF get_point(int index);
		void clear();
		void clearViz();
		void plot(bool special = false );
};
